from naoqi import ALProxy
import time
move = ALProxy("ALMotion","192.168.1.33",9559)
posture = ALProxy("ALRobotPosture","192.168.1.33",9559)
move.wakeUp()
posture.goToPosture("StandInit",0.5)

x = 1
y = 0.0
theta = -0.02

maxstepx1 = 0.07
maxstepx2 = 0.06
maxstepy1 = 0.14
maxstepy2 = 0.13
maxsteptheta = 0.4
maxstepfrequency1 = 0.5
maxstepfrequency2 = 0.7 
stepheight = 0.016
torsowx = 0.0
torsowy = 0.0
moveConfig1 = [["MaxStepX",maxstepx1],
                ["MaxStepY",maxstepy1],
                ["MaxStepTheta",maxsteptheta],
                ["MaxStepFrequency",maxstepfrequency1],
                ["StepHeight",stepheight],
                ["TorsoWx",torsowx],
                ["TorsoWy",torsowy]]
moveConfig2 = [["MaxStepX",maxstepx2],
                ["MaxStepY",maxstepy2],
                ["MaxStepTheta",maxsteptheta],
                ["MaxStepFrequency",maxstepfrequency2],
                ["StepHeight",stepheight],
                ["TorsoWx",torsowx],
                ["TorsoWy",torsowy]]
try:
    move.angleInterpolation("HeadPitch",0.5,1,False)
    move.angleInterpolation("HeadYaw", -0.00, 1, False)
    move.move(x-0.6,y,theta,moveConfig1)
    time.sleep(3)
    move.move(x+4, y, theta+0.02, moveConfig2)
    time.sleep(3)
    move.move(x, y, theta, moveConfig2)
   
   # time.sleep(35)
   #move.stopMove()
   #move.rest()
except Exception, errorMsg:
    print str(errorMsg)
    print "This example is not allowed on this robot."
    exit()
